<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.11"/>
<title>CL-CBS: cl_cbs.cpp</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<script type="text/javascript">
  $(document).ready(function() { init_search(); });
</script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">CL-CBS
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.11 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="namespaces.html"><span>Namespaces</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li><a href="files.html"><span>Files</span></a></li>
      <li><a href="examples.html"><span>Examples</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
        <span class="left">
          <img id="MSearchSelect" src="search/mag_sel.png"
               onmouseover="return searchBox.OnSearchSelectShow()"
               onmouseout="return searchBox.OnSearchSelectHide()"
               alt=""/>
          <input type="text" id="MSearchField" value="Search" accesskey="S"
               onfocus="searchBox.OnSearchFieldFocus(true)" 
               onblur="searchBox.OnSearchFieldFocus(false)" 
               onkeyup="searchBox.OnSearchFieldChange(event)"/>
          </span><span class="right">
            <a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="search/close.png" alt=""/></a>
          </span>
        </div>
      </li>
    </ul>
  </div>
</div><!-- top -->
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div class="header">
  <div class="headertitle">
<div class="title">cl_cbs.cpp</div>  </div>
</div><!--header-->
<div class="contents">
<p>Implementation of whole CL-CBS method</p>
<div class="fragment"><div class="line"></div><div class="line"><span class="preprocessor">#include &quot;<a class="code" href="cl__cbs_8hpp.html">cl_cbs.hpp</a>&quot;</span></div><div class="line"></div><div class="line"><span class="preprocessor">#include &lt;sys/stat.h&gt;</span></div><div class="line"><span class="preprocessor">#include &lt;unistd.h&gt;</span></div><div class="line"><span class="preprocessor">#include &lt;yaml-cpp/yaml.h&gt;</span></div><div class="line"></div><div class="line"><span class="preprocessor">#include &lt;boost/functional/hash.hpp&gt;</span></div><div class="line"><span class="preprocessor">#include &lt;boost/numeric/ublas/matrix.hpp&gt;</span></div><div class="line"><span class="preprocessor">#include &lt;boost/program_options.hpp&gt;</span></div><div class="line"><span class="preprocessor">#include &lt;fstream&gt;</span></div><div class="line"><span class="preprocessor">#include &lt;iostream&gt;</span></div><div class="line"></div><div class="line"><span class="preprocessor">#include &quot;<a class="code" href="environment_8hpp.html">environment.hpp</a>&quot;</span></div><div class="line"><span class="preprocessor">#include &quot;<a class="code" href="timer_8hpp.html">timer.hpp</a>&quot;</span></div><div class="line"></div><div class="line"><span class="keyword">using</span> <a name="_a0"></a><a class="code" href="classlibMultiRobotPlanning_1_1CL__CBS.html">libMultiRobotPlanning::CL_CBS</a>;</div><div class="line"><span class="keyword">using</span> <a name="_a1"></a><a class="code" href="structlibMultiRobotPlanning_1_1Neighbor.html">libMultiRobotPlanning::Neighbor</a>;</div><div class="line"><span class="keyword">using</span> <a name="_a2"></a><a class="code" href="structlibMultiRobotPlanning_1_1PlanResult.html">libMultiRobotPlanning::PlanResult</a>;</div><div class="line"><span class="keyword">using namespace </span><a class="code" href="namespacelibMultiRobotPlanning.html">libMultiRobotPlanning</a>;</div><div class="line"></div><div class="line"><span class="comment">// calculate agent collision more precisely BUT need LONGER time</span></div><div class="line"><span class="comment">// #define PRCISE_COLLISION</span></div><div class="line"></div><div class="line"><span class="keyword">struct </span>Location {</div><div class="line">  Location(<span class="keywordtype">double</span> x, <span class="keywordtype">double</span> y) : x(x), y(y) {}</div><div class="line">  <span class="keywordtype">double</span> x;</div><div class="line">  <span class="keywordtype">double</span> y;</div><div class="line"></div><div class="line">  <span class="keywordtype">bool</span> operator&lt;(<span class="keyword">const</span> Location&amp; other)<span class="keyword"> const </span>{</div><div class="line">    <span class="keywordflow">return</span> std::tie(x, y) &lt; std::tie(other.x, other.y);</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordtype">bool</span> operator==(<span class="keyword">const</span> Location&amp; other)<span class="keyword"> const </span>{</div><div class="line">    <span class="keywordflow">return</span> std::tie(x, y) == std::tie(other.x, other.y);</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keyword">friend</span> std::ostream&amp; operator&lt;&lt;(std::ostream&amp; os, <span class="keyword">const</span> Location&amp; c) {</div><div class="line">    <span class="keywordflow">return</span> os &lt;&lt; <span class="stringliteral">&quot;(&quot;</span> &lt;&lt; c.x &lt;&lt; <span class="stringliteral">&quot;,&quot;</span> &lt;&lt; c.y &lt;&lt; <span class="stringliteral">&quot;)&quot;</span>;</div><div class="line">  }</div><div class="line">};</div><div class="line"></div><div class="line"><span class="keyword">namespace </span>std {</div><div class="line"><span class="keyword">template</span> &lt;&gt;</div><div class="line"><span class="keyword">struct </span>hash&lt;Location&gt; {</div><div class="line">  <span class="keywordtype">size_t</span> operator()(<span class="keyword">const</span> Location&amp; s)<span class="keyword"> const </span>{</div><div class="line">    <span class="keywordtype">size_t</span> seed = 0;</div><div class="line">    boost::hash_combine(seed, s.x);</div><div class="line">    boost::hash_combine(seed, s.y);</div><div class="line">    <span class="keywordflow">return</span> seed;</div><div class="line">  }</div><div class="line">};</div><div class="line">}  <span class="comment">// namespace std</span></div><div class="line"></div><div class="line"><span class="keyword">struct </span>State {</div><div class="line">  State(<span class="keywordtype">double</span> x, <span class="keywordtype">double</span> y, <span class="keywordtype">double</span> yaw, <span class="keywordtype">int</span> time = 0)</div><div class="line">      : time(time), x(x), y(y), yaw(yaw) {</div><div class="line">    rot.resize(2, 2);</div><div class="line">    rot(0, 0) = cos(-this-&gt;yaw);</div><div class="line">    rot(0, 1) = -sin(-this-&gt;yaw);</div><div class="line">    rot(1, 0) = sin(-this-&gt;yaw);</div><div class="line">    rot(1, 1) = cos(-this-&gt;yaw);</div><div class="line"><span class="preprocessor">#ifdef PRCISE_COLLISION</span></div><div class="line">    corner1 = <a name="a3"></a><a class="code" href="namespacelibMultiRobotPlanning.html#ac189dec8584a215824ec333efb32bd41">Point</a>(</div><div class="line">        this-&gt;x -</div><div class="line">            sqrt(pow(Constants::carWidth / 2 * 1.1, 2) +</div><div class="line">                 pow(Constants::LB * 1.1, 2)) *</div><div class="line">                cos(atan2(Constants::carWidth / 2, Constants::LB) - this-&gt;yaw),</div><div class="line">        this-&gt;y -</div><div class="line">            sqrt(pow(Constants::carWidth / 2 * 1.1, 2) +</div><div class="line">                 pow(Constants::LB * 1.1, 2)) *</div><div class="line">                sin(atan2(Constants::carWidth / 2, Constants::LB) - this-&gt;yaw));</div><div class="line">    corner2 = <a class="code" href="namespacelibMultiRobotPlanning.html#ac189dec8584a215824ec333efb32bd41">Point</a>(</div><div class="line">        this-&gt;x -</div><div class="line">            sqrt(pow(Constants::carWidth / 2 * 1.1, 2) +</div><div class="line">                 pow(Constants::LB * 1.1, 2)) *</div><div class="line">                cos(atan2(Constants::carWidth / 2, Constants::LB) + this-&gt;yaw),</div><div class="line">        this-&gt;y +</div><div class="line">            sqrt(pow(Constants::carWidth / 2 * 1.1, 2) +</div><div class="line">                 pow(Constants::LB * 1.1, 2)) *</div><div class="line">                sin(atan2(Constants::carWidth / 2, Constants::LB) + this-&gt;yaw));</div><div class="line">    corner3 = <a class="code" href="namespacelibMultiRobotPlanning.html#ac189dec8584a215824ec333efb32bd41">Point</a>(</div><div class="line">        this-&gt;x +</div><div class="line">            sqrt(pow(Constants::carWidth / 2 * 1.1, 2) +</div><div class="line">                 pow(Constants::LF * 1.1, 2)) *</div><div class="line">                cos(atan2(Constants::carWidth / 2, Constants::LF) - this-&gt;yaw),</div><div class="line">        this-&gt;y +</div><div class="line">            sqrt(pow(Constants::carWidth / 2 * 1.1, 2) +</div><div class="line">                 pow(Constants::LF * 1.1, 2)) *</div><div class="line">                sin(atan2(Constants::carWidth / 2, Constants::LF) - this-&gt;yaw));</div><div class="line">    corner4 = <a class="code" href="namespacelibMultiRobotPlanning.html#ac189dec8584a215824ec333efb32bd41">Point</a>(</div><div class="line">        this-&gt;x +</div><div class="line">            sqrt(pow(Constants::carWidth / 2 * 1.1, 2) +</div><div class="line">                 pow(Constants::LF * 1.1, 2)) *</div><div class="line">                cos(atan2(Constants::carWidth / 2, Constants::LF) + this-&gt;yaw),</div><div class="line">        this-&gt;y -</div><div class="line">            sqrt(pow(Constants::carWidth / 2 * 1.1, 2) +</div><div class="line">                 pow(Constants::LF * 1.1, 2)) *</div><div class="line">                sin(atan2(Constants::carWidth / 2, Constants::LF) + this-&gt;yaw));</div><div class="line"><span class="preprocessor">#endif</span></div><div class="line">  }</div><div class="line"></div><div class="line">  State() = <span class="keywordflow">default</span>;</div><div class="line"></div><div class="line">  <span class="keywordtype">bool</span> operator==(<span class="keyword">const</span> State&amp; s)<span class="keyword"> const </span>{</div><div class="line">    <span class="keywordflow">return</span> std::tie(time, x, y, yaw) == std::tie(s.time, s.x, s.y, s.yaw);</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordtype">bool</span> agentCollision(<span class="keyword">const</span> State&amp; other)<span class="keyword"> const </span>{</div><div class="line"><span class="preprocessor">#ifndef PRCISE_COLLISION</span></div><div class="line">    <span class="keywordflow">if</span> (pow(this-&gt;x - other.x, 2) + pow(this-&gt;y - other.y, 2) &lt;</div><div class="line">        pow(2 * Constants::LF, 2) + pow(Constants::carWidth, 2))</div><div class="line">      <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line">    <span class="keywordflow">return</span> <span class="keyword">false</span>;</div><div class="line"><span class="preprocessor">#else</span></div><div class="line">    std::vector&lt;Segment&gt; rectangle1{<a name="a4"></a><a class="code" href="namespacelibMultiRobotPlanning.html#a1b9a5d62ba2a055760096d0fb035f8c9">Segment</a>(this-&gt;corner1, this-&gt;corner2),</div><div class="line">                                    <a class="code" href="namespacelibMultiRobotPlanning.html#a1b9a5d62ba2a055760096d0fb035f8c9">Segment</a>(this-&gt;corner2, this-&gt;corner3),</div><div class="line">                                    <a class="code" href="namespacelibMultiRobotPlanning.html#a1b9a5d62ba2a055760096d0fb035f8c9">Segment</a>(this-&gt;corner3, this-&gt;corner4),</div><div class="line">                                    <a class="code" href="namespacelibMultiRobotPlanning.html#a1b9a5d62ba2a055760096d0fb035f8c9">Segment</a>(this-&gt;corner4, this-&gt;corner1)};</div><div class="line">    std::vector&lt;Segment&gt; rectangle2{<a class="code" href="namespacelibMultiRobotPlanning.html#a1b9a5d62ba2a055760096d0fb035f8c9">Segment</a>(other.corner1, other.corner2),</div><div class="line">                                    <a class="code" href="namespacelibMultiRobotPlanning.html#a1b9a5d62ba2a055760096d0fb035f8c9">Segment</a>(other.corner2, other.corner3),</div><div class="line">                                    <a class="code" href="namespacelibMultiRobotPlanning.html#a1b9a5d62ba2a055760096d0fb035f8c9">Segment</a>(other.corner3, other.corner4),</div><div class="line">                                    <a class="code" href="namespacelibMultiRobotPlanning.html#a1b9a5d62ba2a055760096d0fb035f8c9">Segment</a>(other.corner4, other.corner1)};</div><div class="line">    <span class="keywordflow">for</span> (<span class="keyword">auto</span> seg1 = rectangle1.begin(); seg1 != rectangle1.end(); seg1++)</div><div class="line">      <span class="keywordflow">for</span> (<span class="keyword">auto</span> seg2 = rectangle2.begin(); seg2 != rectangle2.end(); seg2++) {</div><div class="line">        <span class="keywordflow">if</span> (boost::geometry::intersects(*seg1, *seg2)) <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line">      }</div><div class="line">    <span class="keywordflow">return</span> <span class="keyword">false</span>;</div><div class="line"><span class="preprocessor">#endif</span></div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordtype">bool</span> obsCollision(<span class="keyword">const</span> Location&amp; obstacle)<span class="keyword"> const </span>{</div><div class="line">    boost::numeric::ublas::matrix&lt;double&gt; obs(1, 2);</div><div class="line">    obs(0, 0) = obstacle.x - this-&gt;x;</div><div class="line">    obs(0, 1) = obstacle.y - this-&gt;y;</div><div class="line"></div><div class="line">    <span class="keyword">auto</span> rotated_obs = boost::numeric::ublas::prod(obs, rot);</div><div class="line">    <span class="keywordflow">if</span> (rotated_obs(0, 0) &gt; -Constants::LB - Constants::obsRadius &amp;&amp;</div><div class="line">        rotated_obs(0, 0) &lt; Constants::LF + Constants::obsRadius &amp;&amp;</div><div class="line">        rotated_obs(0, 1) &gt; -Constants::carWidth / 2.0 - Constants::obsRadius &amp;&amp;</div><div class="line">        rotated_obs(0, 1) &lt; Constants::carWidth / 2.0 + Constants::obsRadius)</div><div class="line">      <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line">    <span class="keywordflow">return</span> <span class="keyword">false</span>;</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keyword">friend</span> std::ostream&amp; operator&lt;&lt;(std::ostream&amp; os, <span class="keyword">const</span> State&amp; s) {</div><div class="line">    <span class="keywordflow">return</span> os &lt;&lt; <span class="stringliteral">&quot;(&quot;</span> &lt;&lt; s.x &lt;&lt; <span class="stringliteral">&quot;,&quot;</span> &lt;&lt; s.y &lt;&lt; <span class="stringliteral">&quot;:&quot;</span> &lt;&lt; s.yaw &lt;&lt; <span class="stringliteral">&quot;)@&quot;</span> &lt;&lt; s.time;</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordtype">int</span> time;</div><div class="line">  <span class="keywordtype">double</span> x;</div><div class="line">  <span class="keywordtype">double</span> y;</div><div class="line">  <span class="keywordtype">double</span> yaw;</div><div class="line"></div><div class="line"> <span class="keyword">private</span>:</div><div class="line">  boost::numeric::ublas::matrix&lt;double&gt; rot;</div><div class="line">  <a class="code" href="namespacelibMultiRobotPlanning.html#ac189dec8584a215824ec333efb32bd41">Point</a> corner1, corner2, corner3, corner4;</div><div class="line">};</div><div class="line"></div><div class="line"><span class="keyword">namespace </span>std {</div><div class="line"><span class="keyword">template</span> &lt;&gt;</div><div class="line"><span class="keyword">struct </span>hash&lt;State&gt; {</div><div class="line">  <span class="keywordtype">size_t</span> operator()(<span class="keyword">const</span> State&amp; s)<span class="keyword"> const </span>{</div><div class="line">    <span class="keywordtype">size_t</span> seed = 0;</div><div class="line">    boost::hash_combine(seed, s.time);</div><div class="line">    boost::hash_combine(seed, s.x);</div><div class="line">    boost::hash_combine(seed, s.y);</div><div class="line">    boost::hash_combine(seed, s.yaw);</div><div class="line">    <span class="keywordflow">return</span> seed;</div><div class="line">  }</div><div class="line">};</div><div class="line">}  <span class="comment">// namespace std</span></div><div class="line"></div><div class="line"><span class="keyword">using</span> Action = int;  <span class="comment">// int&lt;7 int ==6 wait</span></div><div class="line"></div><div class="line"><span class="keyword">struct </span>Conflict {</div><div class="line">  <span class="keywordtype">int</span> time;</div><div class="line">  <span class="keywordtype">size_t</span> agent1;</div><div class="line">  <span class="keywordtype">size_t</span> agent2;</div><div class="line"></div><div class="line">  State s1;</div><div class="line">  State s2;</div><div class="line"></div><div class="line">  <span class="keyword">friend</span> std::ostream&amp; operator&lt;&lt;(std::ostream&amp; os, <span class="keyword">const</span> Conflict&amp; c) {</div><div class="line">    os &lt;&lt; c.time &lt;&lt; <span class="stringliteral">&quot;: Collision [ &quot;</span> &lt;&lt; c.agent1 &lt;&lt; c.s1 &lt;&lt; <span class="stringliteral">&quot; , &quot;</span> &lt;&lt; c.agent2</div><div class="line">       &lt;&lt; c.s2 &lt;&lt; <span class="stringliteral">&quot; ]&quot;</span>;</div><div class="line">    <span class="keywordflow">return</span> os;</div><div class="line">  }</div><div class="line">};</div><div class="line"></div><div class="line"><span class="keyword">struct </span>Constraint {</div><div class="line">  Constraint(<span class="keywordtype">int</span> time, State s, <span class="keywordtype">size_t</span> agentid)</div><div class="line">      : time(time), s(s), agentid(agentid) {}</div><div class="line">  Constraint() = <span class="keywordflow">default</span>;</div><div class="line">  <span class="keywordtype">int</span> time;</div><div class="line">  State s;</div><div class="line">  <span class="keywordtype">size_t</span> agentid;</div><div class="line"></div><div class="line">  <span class="keywordtype">bool</span> operator&lt;(<span class="keyword">const</span> Constraint&amp; other)<span class="keyword"> const </span>{</div><div class="line">    <span class="keywordflow">return</span> std::tie(time, s.x, s.y, s.yaw, agentid) &lt;</div><div class="line">           std::tie(other.time, other.s.x, other.s.y, other.s.yaw,</div><div class="line">                    other.agentid);</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordtype">bool</span> operator==(<span class="keyword">const</span> Constraint&amp; other)<span class="keyword"> const </span>{</div><div class="line">    <span class="keywordflow">return</span> std::tie(time, s.x, s.y, s.yaw, agentid) ==</div><div class="line">           std::tie(other.time, other.s.x, other.s.y, other.s.yaw,</div><div class="line">                    other.agentid);</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keyword">friend</span> std::ostream&amp; operator&lt;&lt;(std::ostream&amp; os, <span class="keyword">const</span> Constraint&amp; c) {</div><div class="line">    <span class="keywordflow">return</span> os &lt;&lt; <span class="stringliteral">&quot;Constraint[&quot;</span> &lt;&lt; c.time &lt;&lt; <span class="stringliteral">&quot;,&quot;</span> &lt;&lt; c.s &lt;&lt; <span class="stringliteral">&quot;from &quot;</span> &lt;&lt; c.agentid</div><div class="line">              &lt;&lt; <span class="stringliteral">&quot;]&quot;</span>;</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordtype">bool</span> satisfyConstraint(<span class="keyword">const</span> State&amp; state)<span class="keyword"> const </span>{</div><div class="line">    <span class="keywordflow">if</span> (state.time &lt; this-&gt;time ||</div><div class="line">        state.time &gt; this-&gt;time + Constants::constraintWaitTime)</div><div class="line">      <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line">    <span class="keywordflow">return</span> !this-&gt;s.agentCollision(state);</div><div class="line">  }</div><div class="line">};</div><div class="line"></div><div class="line"><span class="keyword">namespace </span>std {</div><div class="line"><span class="keyword">template</span> &lt;&gt;</div><div class="line"><span class="keyword">struct </span>hash&lt;Constraint&gt; {</div><div class="line">  <span class="keywordtype">size_t</span> operator()(<span class="keyword">const</span> Constraint&amp; s)<span class="keyword"> const </span>{</div><div class="line">    <span class="keywordtype">size_t</span> seed = 0;</div><div class="line">    boost::hash_combine(seed, s.time);</div><div class="line">    boost::hash_combine(seed, s.s.x);</div><div class="line">    boost::hash_combine(seed, s.s.y);</div><div class="line">    boost::hash_combine(seed, s.s.yaw);</div><div class="line">    boost::hash_combine(seed, s.agentid);</div><div class="line">    <span class="keywordflow">return</span> seed;</div><div class="line">  }</div><div class="line">};</div><div class="line">}  <span class="comment">// namespace std</span></div><div class="line"></div><div class="line"><span class="comment">// FIXME: modidy data struct, it&#39;s not the best option</span></div><div class="line"><span class="keyword">struct </span>Constraints {</div><div class="line">  std::unordered_set&lt;Constraint&gt; constraints;</div><div class="line"></div><div class="line">  <span class="keywordtype">void</span> add(<span class="keyword">const</span> Constraints&amp; other) {</div><div class="line">    constraints.insert(other.constraints.begin(), other.constraints.end());</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordtype">bool</span> overlap(<span class="keyword">const</span> Constraints&amp; other) {</div><div class="line">    <span class="keywordflow">for</span> (<span class="keyword">const</span> <span class="keyword">auto</span>&amp; c : constraints) {</div><div class="line">      <span class="keywordflow">if</span> (other.constraints.count(c) &gt; 0) <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line">    }</div><div class="line">    <span class="keywordflow">return</span> <span class="keyword">false</span>;</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keyword">friend</span> std::ostream&amp; operator&lt;&lt;(std::ostream&amp; os, <span class="keyword">const</span> Constraints&amp; cs) {</div><div class="line">    <span class="keywordflow">for</span> (<span class="keyword">const</span> <span class="keyword">auto</span>&amp; c : cs.constraints) {</div><div class="line">      os &lt;&lt; c &lt;&lt; std::endl;</div><div class="line">    }</div><div class="line">    <span class="keywordflow">return</span> os;</div><div class="line">  }</div><div class="line">};</div><div class="line"></div><div class="line"><span class="keywordtype">void</span> readAgentConfig() {</div><div class="line">  YAML::Node car_config;</div><div class="line">  std::string test(__FILE__);</div><div class="line">  boost::replace_all(test, <span class="stringliteral">&quot;cl_cbs.cpp&quot;</span>, <span class="stringliteral">&quot;config.yaml&quot;</span>);</div><div class="line">  <span class="keywordflow">try</span> {</div><div class="line">    car_config = YAML::LoadFile(test.c_str());</div><div class="line">  } <span class="keywordflow">catch</span> (std::exception&amp; e) {</div><div class="line">    std::cerr &lt;&lt; <span class="stringliteral">&quot;\033[1m\033[33mWARNING: Failed to load agent config file: &quot;</span></div><div class="line">              &lt;&lt; test &lt;&lt; <span class="stringliteral">&quot;\033[0m , Using default params. \n&quot;</span>;</div><div class="line">  }</div><div class="line">  <span class="comment">// int car_r = car_config[&quot;r&quot;].as&lt;int&gt;();</span></div><div class="line">  Constants::r = car_config[<span class="stringliteral">&quot;r&quot;</span>].as&lt;<span class="keywordtype">double</span>&gt;();</div><div class="line">  Constants::deltat = car_config[<span class="stringliteral">&quot;deltat&quot;</span>].as&lt;<span class="keywordtype">double</span>&gt;();</div><div class="line">  Constants::penaltyTurning = car_config[<span class="stringliteral">&quot;penaltyTurning&quot;</span>].as&lt;<span class="keywordtype">double</span>&gt;();</div><div class="line">  Constants::penaltyReversing = car_config[<span class="stringliteral">&quot;penaltyTurning&quot;</span>].as&lt;<span class="keywordtype">double</span>&gt;();</div><div class="line">  Constants::penaltyCOD = car_config[<span class="stringliteral">&quot;penaltyTurning&quot;</span>].as&lt;<span class="keywordtype">double</span>&gt;();</div><div class="line">  <span class="comment">// map resolution</span></div><div class="line">  Constants::mapResolution = car_config[<span class="stringliteral">&quot;mapResolution&quot;</span>].as&lt;<span class="keywordtype">double</span>&gt;();</div><div class="line">  <span class="comment">// change to set calcIndex resolution</span></div><div class="line">  Constants::xyResolution = Constants::r * Constants::deltat;</div><div class="line">  Constants::yawResolution = Constants::deltat;</div><div class="line"></div><div class="line">  Constants::carWidth = car_config[<span class="stringliteral">&quot;carWidth&quot;</span>].as&lt;<span class="keywordtype">double</span>&gt;();</div><div class="line">  Constants::LF = car_config[<span class="stringliteral">&quot;LF&quot;</span>].as&lt;<span class="keywordtype">double</span>&gt;();</div><div class="line">  Constants::LB = car_config[<span class="stringliteral">&quot;LB&quot;</span>].as&lt;<span class="keywordtype">double</span>&gt;();</div><div class="line">  <span class="comment">// obstacle default radius</span></div><div class="line">  Constants::obsRadius = car_config[<span class="stringliteral">&quot;obsRadius&quot;</span>].as&lt;<span class="keywordtype">double</span>&gt;();</div><div class="line">  <span class="comment">// least time to wait for constraint</span></div><div class="line">  Constants::constraintWaitTime = car_config[<span class="stringliteral">&quot;constraintWaitTime&quot;</span>].as&lt;<span class="keywordtype">double</span>&gt;();</div><div class="line"></div><div class="line">  <a name="a5"></a><a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">Constants::dx</a> = {Constants::r * Constants::deltat,</div><div class="line">                   Constants::r * sin(Constants::deltat),</div><div class="line">                   Constants::r * sin(Constants::deltat),</div><div class="line">                   -Constants::r * Constants::deltat,</div><div class="line">                   -Constants::r * sin(Constants::deltat),</div><div class="line">                   -Constants::r * sin(Constants::deltat)};</div><div class="line">  <a name="a6"></a><a class="code" href="namespaceConstants.html#a51c342708494b61085d51877cf6f1f28">Constants::dy</a> = {0,</div><div class="line">                   -Constants::r * (1 - cos(Constants::deltat)),</div><div class="line">                   Constants::r * (1 - cos(Constants::deltat)),</div><div class="line">                   0,</div><div class="line">                   -Constants::r * (1 - cos(Constants::deltat)),</div><div class="line">                   Constants::r * (1 - cos(Constants::deltat))};</div><div class="line">  <a name="a7"></a><a class="code" href="namespaceConstants.html#a627bafd3befec8fad9e764920e9e56f4">Constants::dyaw</a> = {0, Constants::deltat,  -Constants::deltat,</div><div class="line">                     0, -Constants::deltat, Constants::deltat};</div><div class="line">}</div><div class="line"></div><div class="line"><span class="keywordtype">int</span> main(<span class="keywordtype">int</span> argc, <span class="keywordtype">char</span>* argv[]) {</div><div class="line">  <span class="keyword">namespace </span>po = boost::program_options;</div><div class="line">  <span class="comment">// Declare the supported options.</span></div><div class="line">  po::options_description desc(<span class="stringliteral">&quot;Allowed options&quot;</span>);</div><div class="line">  std::string inputFile;</div><div class="line">  std::string outputFile;</div><div class="line">  <span class="keywordtype">int</span> batchSize;</div><div class="line">  desc.add_options()(<span class="stringliteral">&quot;help&quot;</span>, <span class="stringliteral">&quot;produce help message&quot;</span>)(</div><div class="line">      <span class="stringliteral">&quot;input,i&quot;</span>, po::value&lt;std::string&gt;(&amp;inputFile)-&gt;required(),</div><div class="line">      <span class="stringliteral">&quot;input file (YAML)&quot;</span>)(<span class="stringliteral">&quot;output,o&quot;</span>,</div><div class="line">                           po::value&lt;std::string&gt;(&amp;outputFile)-&gt;required(),</div><div class="line">                           <span class="stringliteral">&quot;output file (YAML)&quot;</span>)(</div><div class="line">      <span class="stringliteral">&quot;batchsize,b&quot;</span>, po::value&lt;int&gt;(&amp;batchSize)-&gt;default_value(10),</div><div class="line">      <span class="stringliteral">&quot;batch size for iter&quot;</span>);</div><div class="line"></div><div class="line">  <span class="keywordflow">try</span> {</div><div class="line">    po::variables_map vm;</div><div class="line">    po::store(po::parse_command_line(argc, argv, desc), vm);</div><div class="line">    po::notify(vm);</div><div class="line"></div><div class="line">    <span class="keywordflow">if</span> (vm.count(<span class="stringliteral">&quot;help&quot;</span>) != 0u) {</div><div class="line">      std::cout &lt;&lt; desc &lt;&lt; <span class="stringliteral">&quot;\n&quot;</span>;</div><div class="line">      <span class="keywordflow">return</span> 0;</div><div class="line">    }</div><div class="line">  } <span class="keywordflow">catch</span> (po::error&amp; e) {</div><div class="line">    std::cerr &lt;&lt; e.what() &lt;&lt; std::endl &lt;&lt; std::endl;</div><div class="line">    std::cerr &lt;&lt; desc &lt;&lt; std::endl;</div><div class="line">    <span class="keywordflow">return</span> 1;</div><div class="line">  }</div><div class="line"></div><div class="line">  readAgentConfig();</div><div class="line"></div><div class="line">  YAML::Node map_config;</div><div class="line">  <span class="keywordflow">try</span> {</div><div class="line">    map_config = YAML::LoadFile(inputFile);</div><div class="line">  } <span class="keywordflow">catch</span> (std::exception&amp; e) {</div><div class="line">    std::cerr &lt;&lt; <span class="stringliteral">&quot;\033[1m\033[31mERROR: Failed to load map file: &quot;</span> &lt;&lt; inputFile</div><div class="line">              &lt;&lt; <span class="stringliteral">&quot;\033[0m \n&quot;</span>;</div><div class="line">    <span class="keywordflow">return</span> 0;</div><div class="line">  }</div><div class="line">  <span class="keyword">const</span> <span class="keyword">auto</span>&amp; dim = map_config[<span class="stringliteral">&quot;map&quot;</span>][<span class="stringliteral">&quot;dimensions&quot;</span>];</div><div class="line">  <span class="keywordtype">int</span> dimx = dim[0].as&lt;<span class="keywordtype">int</span>&gt;();</div><div class="line">  <span class="keywordtype">int</span> dimy = dim[1].as&lt;<span class="keywordtype">int</span>&gt;();</div><div class="line"></div><div class="line">  std::unordered_set&lt;Location&gt; obstacles;</div><div class="line">  std::multimap&lt;int, State&gt; dynamic_obstacles;</div><div class="line">  std::vector&lt;State&gt; goals;</div><div class="line">  std::vector&lt;State&gt; startStates;</div><div class="line">  <span class="keywordflow">for</span> (<span class="keyword">const</span> <span class="keyword">auto</span>&amp; node : map_config[<span class="stringliteral">&quot;map&quot;</span>][<span class="stringliteral">&quot;obstacles&quot;</span>]) {</div><div class="line">    obstacles.insert(Location(node[0].as&lt;double&gt;(), node[1].as&lt;double&gt;()));</div><div class="line">  }</div><div class="line">  <span class="keywordflow">for</span> (<span class="keyword">const</span> <span class="keyword">auto</span>&amp; node : map_config[<span class="stringliteral">&quot;agents&quot;</span>]) {</div><div class="line">    <span class="keyword">const</span> <span class="keyword">auto</span>&amp; start = node[<span class="stringliteral">&quot;start&quot;</span>];</div><div class="line">    <span class="keyword">const</span> <span class="keyword">auto</span>&amp; goal = node[<span class="stringliteral">&quot;goal&quot;</span>];</div><div class="line">    startStates.emplace_back(State(start[0].as&lt;double&gt;(), start[1].as&lt;double&gt;(),</div><div class="line">                                   start[2].as&lt;double&gt;()));</div><div class="line">    <span class="comment">// std::cout &lt;&lt; &quot;s: &quot; &lt;&lt; startStates.back() &lt;&lt; std::endl;</span></div><div class="line">    goals.emplace_back(State(goal[0].as&lt;double&gt;(), goal[1].as&lt;double&gt;(),</div><div class="line">                             goal[2].as&lt;double&gt;()));</div><div class="line">  }</div><div class="line"></div><div class="line">  std::cout &lt;&lt; <span class="stringliteral">&quot;Calculating Solution...\n&quot;</span>;</div><div class="line">  <span class="keywordtype">double</span> timer = 0;</div><div class="line">  <span class="keywordtype">bool</span> success = <span class="keyword">false</span>;</div><div class="line">  std::vector&lt;PlanResult&lt;State, Action, double&gt;&gt; solution;</div><div class="line">  <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> iter = 0; iter &lt; (double)goals.size() / batchSize; iter++) {</div><div class="line">    <span class="keywordtype">size_t</span> first = iter * batchSize;</div><div class="line">    <span class="keywordtype">size_t</span> last = first + batchSize;</div><div class="line">    <span class="keywordflow">if</span> (last &gt;= goals.size()) last = goals.size();</div><div class="line">    std::vector&lt;State&gt; m_goals(goals.begin() + first, goals.begin() + last);</div><div class="line">    std::vector&lt;State&gt; m_starts(startStates.begin() + first,</div><div class="line">                                startStates.begin() + last);</div><div class="line"></div><div class="line">    <a name="_a8"></a><a class="code" href="classlibMultiRobotPlanning_1_1Environment.html">Environment</a>&lt;Location, State, Action, double, Conflict, Constraint,</div><div class="line">                Constraints&gt;</div><div class="line">        mapf(dimx, dimy, obstacles, dynamic_obstacles, m_goals);</div><div class="line">    <span class="keywordflow">if</span> (!mapf.startAndGoalValid(m_starts, iter, batchSize)) {</div><div class="line">      success = <span class="keyword">false</span>;</div><div class="line">      <span class="keywordflow">break</span>;</div><div class="line">    }</div><div class="line">    <span class="keywordflow">for</span> (<span class="keyword">auto</span> goal = goals.begin() + last; goal != goals.end(); goal++) {</div><div class="line">      dynamic_obstacles.insert(</div><div class="line">          std::pair&lt;int, State&gt;(-1, State(goal-&gt;x, goal-&gt;y, goal-&gt;yaw)));</div><div class="line">    }</div><div class="line">    <a class="code" href="classlibMultiRobotPlanning_1_1CL__CBS.html">CL_CBS</a>&lt;State, Action, double, Conflict, Constraints,</div><div class="line">           <a class="code" href="classlibMultiRobotPlanning_1_1Environment.html">Environment</a>&lt;Location, State, Action, double, Conflict, Constraint,</div><div class="line">                       Constraints&gt;&gt;</div><div class="line">        cbsHybrid(mapf);</div><div class="line">    std::vector&lt;PlanResult&lt;State, Action, double&gt;&gt; m_solution;</div><div class="line">    <a name="_a9"></a><a class="code" href="classTimer.html">Timer</a> iterTimer;</div><div class="line">    success = cbsHybrid.search(m_starts, m_solution);</div><div class="line">    iterTimer.<a name="a10"></a><a class="code" href="classTimer.html#a63f0eb44b27402196590a03781515dba">stop</a>();</div><div class="line"></div><div class="line">    <span class="keywordflow">if</span> (!success) {</div><div class="line">      std::cout &lt;&lt; <span class="stringliteral">&quot;\033[1m\033[31m No.&quot;</span> &lt;&lt; iter</div><div class="line">                &lt;&lt; <span class="stringliteral">&quot;iter fail to find a solution \033[0m\n&quot;</span>;</div><div class="line">      <span class="keywordflow">break</span>;</div><div class="line">    } <span class="keywordflow">else</span> {</div><div class="line">      solution.insert(solution.end(), m_solution.begin(), m_solution.end());</div><div class="line">      <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> a = 0; a &lt; m_solution.size(); ++a) {</div><div class="line">        <span class="keywordflow">for</span> (<span class="keyword">const</span> <span class="keyword">auto</span>&amp; state : m_solution[a].states)</div><div class="line">          dynamic_obstacles.insert(std::pair&lt;int, State&gt;(</div><div class="line">              state.first.time,</div><div class="line">              State(state.first.x, state.first.y, state.first.yaw)));</div><div class="line">        State lastState = m_solution[a].states.back().first;</div><div class="line">        dynamic_obstacles.insert(std::pair&lt;int, State&gt;(</div><div class="line">            -lastState.time, State(lastState.x, lastState.y, lastState.yaw)));</div><div class="line">      }</div><div class="line">      timer += iterTimer.<a name="a11"></a><a class="code" href="classTimer.html#a4415a31ce4efa5af7889d91c6eb9dc17">elapsedSeconds</a>();</div><div class="line">      std::cout &lt;&lt; <span class="stringliteral">&quot;Complete &quot;</span> &lt;&lt; iter</div><div class="line">                &lt;&lt; <span class="stringliteral">&quot; iter. Runtime:&quot;</span> &lt;&lt; iterTimer.<a class="code" href="classTimer.html#a4415a31ce4efa5af7889d91c6eb9dc17">elapsedSeconds</a>()</div><div class="line">                &lt;&lt; <span class="stringliteral">&quot; Expand high-level nodes:&quot;</span> &lt;&lt; mapf.highLevelExpanded()</div><div class="line">                &lt;&lt; <span class="stringliteral">&quot; Average Low-level-search time:&quot;</span></div><div class="line">                &lt;&lt; iterTimer.<a class="code" href="classTimer.html#a4415a31ce4efa5af7889d91c6eb9dc17">elapsedSeconds</a>() / mapf.highLevelExpanded() /</div><div class="line">                       m_goals.size()</div><div class="line">                &lt;&lt; std::endl;</div><div class="line">    }</div><div class="line">    dynamic_obstacles.erase(-1);</div><div class="line">  }</div><div class="line"></div><div class="line">  std::ofstream out;</div><div class="line">  out = std::ofstream(outputFile);</div><div class="line"></div><div class="line">  <span class="keywordflow">if</span> (success) {</div><div class="line">    std::cout &lt;&lt; <span class="stringliteral">&quot;\033[1m\033[32m Successfully find solution! \033[0m\n&quot;</span>;</div><div class="line"></div><div class="line">    <span class="keywordtype">double</span> makespan = 0, flowtime = 0, cost = 0;</div><div class="line">    <span class="keywordflow">for</span> (<span class="keyword">const</span> <span class="keyword">auto</span>&amp; s : solution) cost += s.cost;</div><div class="line"></div><div class="line">    <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> a = 0; a &lt; solution.size(); ++a) {</div><div class="line">      <span class="comment">// calculate makespan</span></div><div class="line">      <span class="keywordtype">double</span> current_makespan = 0;</div><div class="line">      <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; solution[a].actions.size(); ++i) {</div><div class="line">        <span class="comment">// some action cost have penalty coefficient</span></div><div class="line"></div><div class="line">        <span class="keywordflow">if</span> (solution[a].actions[i].second &lt; <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">Constants::dx</a>[0])</div><div class="line">          current_makespan += solution[a].actions[i].second;</div><div class="line">        <span class="keywordflow">else</span> <span class="keywordflow">if</span> (solution[a].actions[i].first % 3 == 0)</div><div class="line">          current_makespan += <a class="code" href="namespaceConstants.html#a73c1390c57181cef88dd82781e2e6505">Constants::dx</a>[0];</div><div class="line">        <span class="keywordflow">else</span></div><div class="line">          current_makespan += Constants::r * Constants::deltat;</div><div class="line">      }</div><div class="line">      flowtime += current_makespan;</div><div class="line">      <span class="keywordflow">if</span> (current_makespan &gt; makespan) makespan = current_makespan;</div><div class="line">    }</div><div class="line">    std::cout &lt;&lt; <span class="stringliteral">&quot; Runtime: &quot;</span> &lt;&lt; timer &lt;&lt; std::endl</div><div class="line">              &lt;&lt; <span class="stringliteral">&quot; Makespan:&quot;</span> &lt;&lt; makespan &lt;&lt; std::endl</div><div class="line">              &lt;&lt; <span class="stringliteral">&quot; Flowtime:&quot;</span> &lt;&lt; flowtime &lt;&lt; std::endl</div><div class="line">              &lt;&lt; <span class="stringliteral">&quot; cost:&quot;</span> &lt;&lt; cost &lt;&lt; std::endl;</div><div class="line">    <span class="comment">// output to file</span></div><div class="line">    out &lt;&lt; <span class="stringliteral">&quot;statistics:&quot;</span> &lt;&lt; std::endl;</div><div class="line">    out &lt;&lt; <span class="stringliteral">&quot;  cost: &quot;</span> &lt;&lt; cost &lt;&lt; std::endl;</div><div class="line">    out &lt;&lt; <span class="stringliteral">&quot;  makespan: &quot;</span> &lt;&lt; makespan &lt;&lt; std::endl;</div><div class="line">    out &lt;&lt; <span class="stringliteral">&quot;  flowtime: &quot;</span> &lt;&lt; flowtime &lt;&lt; std::endl;</div><div class="line">    out &lt;&lt; <span class="stringliteral">&quot;  runtime: &quot;</span> &lt;&lt; timer &lt;&lt; std::endl;</div><div class="line">    out &lt;&lt; <span class="stringliteral">&quot;schedule:&quot;</span> &lt;&lt; std::endl;</div><div class="line">    <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> a = 0; a &lt; solution.size(); ++a) {</div><div class="line">      <span class="comment">// std::cout &lt;&lt; &quot;Solution for: &quot; &lt;&lt; a &lt;&lt; std::endl;</span></div><div class="line">      <span class="comment">// for (size_t i = 0; i &lt; solution[a].actions.size(); ++i) {</span></div><div class="line">      <span class="comment">//   std::cout &lt;&lt; solution[a].states[i].second &lt;&lt; &quot;: &quot;</span></div><div class="line">      <span class="comment">//             &lt;&lt; solution[a].states[i].first &lt;&lt; &quot;-&gt;&quot;</span></div><div class="line">      <span class="comment">//             &lt;&lt; solution[a].actions[i].first</span></div><div class="line">      <span class="comment">//             &lt;&lt; &quot;(cost: &quot; &lt;&lt; solution[a].actions[i].second &lt;&lt; &quot;)&quot;</span></div><div class="line">      <span class="comment">//             &lt;&lt; std::endl;</span></div><div class="line">      <span class="comment">// }</span></div><div class="line">      <span class="comment">// std::cout &lt;&lt; solution[a].states.back().second &lt;&lt; &quot;: &quot;</span></div><div class="line">      <span class="comment">//           &lt;&lt; solution[a].states.back().first &lt;&lt; std::endl;</span></div><div class="line"></div><div class="line">      out &lt;&lt; <span class="stringliteral">&quot;  agent&quot;</span> &lt;&lt; a &lt;&lt; <span class="stringliteral">&quot;:&quot;</span> &lt;&lt; std::endl;</div><div class="line">      <span class="keywordflow">for</span> (<span class="keyword">const</span> <span class="keyword">auto</span>&amp; state : solution[a].states) {</div><div class="line">        out &lt;&lt; <span class="stringliteral">&quot;    - x: &quot;</span> &lt;&lt; state.first.x &lt;&lt; std::endl</div><div class="line">            &lt;&lt; <span class="stringliteral">&quot;      y: &quot;</span> &lt;&lt; state.first.y &lt;&lt; std::endl</div><div class="line">            &lt;&lt; <span class="stringliteral">&quot;      yaw: &quot;</span> &lt;&lt; state.first.yaw &lt;&lt; std::endl</div><div class="line">            &lt;&lt; <span class="stringliteral">&quot;      t: &quot;</span> &lt;&lt; state.first.time &lt;&lt; std::endl;</div><div class="line">      }</div><div class="line">    }</div><div class="line">  } <span class="keywordflow">else</span> {</div><div class="line">    std::cout &lt;&lt; <span class="stringliteral">&quot;\033[1m\033[31m Fail to find paths \033[0m\n&quot;</span>;</div><div class="line">  }</div><div class="line">}</div></div><!-- fragment --> </div><!-- contents -->
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
Generated by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.8.11
</small></address>
</body>
</html>
